// RT-Thread 头文件
#include <rtthread.h>
#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include <rtdevice.h>
#include <board.h>

// HAL库头文件
#include "main.h"

// 相关外设头文件
#include "../hardware/LED/led.h"                        // LED         --rt_thread库
#include "../hardware/KEY/key.h"                        // KEY         --rt_thread库
#include "../hardware/PWM/fly_pwm.h"                    // PWM         --HAL库
#include "../hardware/USART_ALL/usart_all.h"            // Serial      --rt_thread库
#include "../hardware/IIC_BMP280/iic_bmp280.h"          // BMP280      --HAL库
#include "../hardware/MS53L/ms53l.h"                    // MS53L       --rt_thread库/HAL库  10hz--0.1m/s  长距离2m
#include "../hardware/JY61P/JY61P.h"                    // JY61P       --rt_thread库
#include "../hardware/MP3m/mp3m.h"                      // MP3         --rt_thread库
#include "../hardware/RGB/rgb_pwm.h"                    // RGB         --HAL库
#include "../hardware/Flight_control/fly_control.h"     // 飞控
#include "../applications/De_bug_mouse/de_bug_mouse.h"  // 相关辅助函数

//c库
#include <math.h>
#include <stdio.h>

// 线程相关头文件
#include "rt_thread_demo.h"

// 线程句柄
#if RT_THREAD_FLY_DEBUG == 1
rt_thread_t  thread1_1;     // LED
rt_thread_t  thread2_1;     // KEY
#endif

rt_thread_t thread1_JY901;          // JY901数据采集
rt_thread_t thread2_PID;            // PID控制
rt_thread_t thread3_MS53L_BMP280;   // MS53L和BMP280传感器数据采集
rt_thread_t thread4_USER;           // 遥控器/调试
rt_thread_t thread5_MP3;            // MP3语音模块
rt_thread_t thread6_RGB;            // RGB灯控制
rt_thread_t thread7_START;          // 解锁线程


// 事件集
#if RT_THREAD_FLY_DEBUG == 1
rt_event_t event_led;
#endif

// 信号量
#if RT_THREAD_FLY_DEBUG == 1
rt_sem_t key_sem;           // 按钮按下释放
#endif

struct rt_semaphore jy901_sem; // JY901数据包信号量
struct rt_semaphore ms53l_sem; // MS53L数据包信号量
struct rt_semaphore pid_sem;   // JY901数据更新后通知PID线程

// 设备(串口)  -- 全部使用中断
rt_device_t uart2_dev;
rt_device_t uart3_dev;
rt_device_t uart4_dev;
rt_device_t uart5_dev;

// 优先级定义(其余参数在init函数中设置)
#if RT_THREAD_FLY_DEBUG == 1
#define priority_led 15     // led
#define priority_key 14     // key
#endif

#define priority1_JY901         6   // JY901
#define priority2_PID           5   // PID控制
#define priority3_MS53L_BMP280  7   // MS53L和BMP280传感器数据采集
#define priority4_USER          8   // 遥控器/调试
#define priority5_MP3           9   // MP3语音模块
#define priority6_RGB           10  // RGB灯
#define priority7_START         6   // 解锁程序

// 全局FLAG
#if RT_THREAD_FLY_DEBUG == 1
extern uint8_t LED1_flag;
extern uint8_t LED2_flag;
#endif

float fusion_height = 0;     //  MS53L和BMP280传感器融合后的高度，单位mm

/****************************** thread_t_init BEGIN ****************************
 * 函    数：thread_t_init
 * 功    能：初始化所有线程
 * 参    数：无
 * 返 回 值：无
 * 注意事项：配置线程参数
 ******************************************************************************/
void thread_t_init()
{
#if RT_THREAD_FLY_DEBUG == 1
    // thread1_1 LED1
    thread1_1 = rt_thread_create("thread1_led1", entry_led, RT_NULL, 512, priority_led, 5);
    if(thread1_1 != RT_NULL) // 创建成功
    {
        rt_thread_startup(thread1_1);
    }
    else rt_kprintf("thread1_1 creat Failede!\n");

    // thread2_1 KEY1
    thread2_1 = rt_thread_create("thread2_1_key", entry_key, RT_NULL, 512, priority_key, 5);
    if(thread2_1 != RT_NULL) // 创建成功
    {
        rt_thread_startup(thread2_1);
    }
    else rt_kprintf("thread2_1 creat Failede!\n");
#endif
    // thread1_JY901
    thread1_JY901 = rt_thread_create("thread1_JY901", entry1_JY901, RT_NULL, 1024, priority1_JY901, 5);
    if(thread1_JY901 != RT_NULL) // 创建成功
    {
        rt_thread_startup(thread1_JY901);
    }
    else rt_kprintf("thread1_JY901 creat Failede!\n");


    // thread2_PID
    thread2_PID = rt_thread_create("thread2_PID", entry2_PID, RT_NULL, 2048, priority2_PID, 10);
    if(thread2_PID != RT_NULL) // 创建成功
    {
        rt_thread_startup(thread2_PID);
    }
    else rt_kprintf("thread2_PID creat Failede!\n");

    // // thread3_MS53L_BMP280
    // thread3_MS53L_BMP280 = rt_thread_create("thread3_MS53L_BMP280", entry3_MS53L_BMP280, RT_NULL, 1024, priority3_MS53L_BMP280, 5);
    // if(thread3_MS53L_BMP280 != RT_NULL) // 创建成功
    // {
    //     rt_thread_startup(thread3_MS53L_BMP280);
    // }
    // else rt_kprintf("thread3_MS53L_BMP280 creat Failede!\n");
    // thread4_USER
    thread4_USER = rt_thread_create("thread4_USER", entry4_USER, RT_NULL, 2048, priority4_USER, 5);
    if(thread4_USER != RT_NULL) // 创建成功
    {
        rt_thread_startup(thread4_USER);
    }
    else rt_kprintf("thread4_USER creat Failede!\n");

//    // thread5_MP3
//    thread5_MP3 = rt_thread_create("thread5_MP3", entry5_MP3, RT_NULL, 512, priority5_MP3, 5);
//    if(thread5_MP3 != RT_NULL) // 创建成功
//    {
//        rt_thread_startup(thread5_MP3);
//    }
//    else rt_kprintf("thread5_MP3 creat Failede!\n");

    // thread6_RGB
    // thread6_RGB = rt_thread_create("thread6_RGB", entry6_RGB, RT_NULL, 1024, priority6_RGB, 5);
    // if(thread6_RGB != RT_NULL) // 创建成功
    // {
    //     rt_thread_startup(thread6_RGB);
    // }
    // else rt_kprintf("thread6_RGB creat Failede!\n");

    // thread7_START
    // thread7_START = rt_thread_create("thread7_START", entry7_START, RT_NULL, 1024, priority7_START, 5);
    // if(thread7_START != RT_NULL) // 创建成功
    // {
    //     rt_thread_startup(thread7_START);
    // }
    // else rt_kprintf("thread7_START creat Failede!\n");
}
/****************************** thread_t_init END ******************************/

/****************************** sem_init BEGIN *********************************
 * 函    数：sem_init
 * 功    能：初始化所有信号量
 * 参    数：无
 * 返 回 值：无
 * 注意事项：配置信号量参数
 ******************************************************************************/
void sem_init()
{
#if RT_THREAD_FLY_DEBUG == 1
    // key_sem
    key_sem = rt_sem_create("key_sem", 0, RT_IPC_FLAG_FIFO);
    if (key_sem == RT_NULL) {
        rt_kprintf("Semaphore create failed!\n");
        return;
    }
#endif
    rt_sem_init(&jy901_sem, "jy901_sem", 0, RT_IPC_FLAG_FIFO); // JY901数据包信号量
    rt_sem_init(&ms53l_sem, "ms53l_sem", 0, RT_IPC_FLAG_FIFO); // MS53L数据包信号量
    rt_sem_init(&pid_sem,   "pid_sem", 0, RT_IPC_FLAG_FIFO);   // JY901数据更新后通知PID线程

}
/****************************** sem_init END ***********************************/

/****************************** event_init BEGIN *******************************
 * 函    数：event_init
 * 功    能：初始化所有事件集
 * 参    数：无
 * 返 回 值：无
 * 注意事项：配置事件集参数
 ******************************************************************************/
void event_init()
{
#if RT_THREAD_FLY_DEBUG == 1
    event_led = rt_event_create("event_led_name", RT_IPC_FLAG_FIFO);
#endif
}
/****************************** event_init END *********************************/

#if RT_THREAD_FLY_DEBUG == 1
/****************************** entry_led BEGIN ********************************
 * 函    数：entry_led
 * 功    能：LED的任务函数，根据标志位更新LED状态——接收事件集并处理两种任务
 * 参    数：无
 * 返 回 值：无
 * 注意事项：注意优先级；
 *           注释掉rt_thread_delay()函数会让LED翻转后直接进入没有接收事件的状态，然后阻塞，会使得键控更灵敏
 ******************************************************************************/
void entry_led(void *p)
{
    rt_uint32_t recv_set = 0; // 用于接收事件标志
    while(1)
    {
#if KEY_LED_SEM == 1
        rt_sem_take(key_sem, RT_WAITING_FOREVER); // 等待信号量（阻塞直到按键触发）
        LED1_flag ^= 0x01; // 翻转
        Update_LED();      // 更新LED状态
#else
        // 等待事件集event_led的第0位或第1位，任一事件发生即可触发，接收后清除事件标志
        rt_event_recv(event_led,
                      LOC(0) | LOC(1),  // 同时等待两个事件标志
                      RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR,  // 使用OR触发，并清除事件标志
                      RT_WAITING_FOREVER,
                      &recv_set);  // 接收到的具体事件标志将保存在recv_set中

        // 判断接收到的具体事件标志
        if (recv_set & LOC(0)) {
            // 按键1按下：翻转LED
            LED1_flag ^= 0x01;
            Update_LED(); // 更新LED状态
        }

        if (recv_set & LOC(1)) {
            // 按键2按下：发送串口消息
        }
//        rt_thread_delay(10)
#endif
    }
}
/****************************** entry_led END **********************************/

/****************************** entry_key BEGIN ********************************
 * 函    数：entry_key
 * 功    能：读取按键/----中断读取
 * 参    数：无
 * 返 回 值：无
 * 注意事项：注意优先级,如果为中断按键，则KEY_Scan不起作用
 ******************************************************************************/
void entry_key(void *p)
{
    while(1)
    {
        if(KEY_Scan(1) == KEY1_PRES) // 按键扫描
        {
#if KEY_LED_SEM == 1
            rt_sem_release(key_sem); // 发送信号量
#else
            rt_event_send(event_led, LOC(0)); // 发送第0位事件集
#endif
        }
        rt_thread_delay(10);
    }
}
/****************************** entry_key END **********************************/
#endif

// JY901 线程入口，负责采集陀螺仪数据并更新全局变量
extern struct rt_semaphore jy901_sem;

void entry1_JY901(void *p)
{
    while(1)
    {
        rt_sem_take(&jy901_sem, RT_WAITING_FOREVER); // 等待新数据包到来
        if(jy901_rx_state_flag == JY61P_GYRO_ID) // 陀螺仪数据包
        {
            gyro_update(); // 更新陀螺仪数据
        }
        else if(jy901_rx_state_flag == JY61P_ACC_ID) // 加速度计数据包
        {
            acc_update(); // 更新加速度计数据
        }
        else if(jy901_rx_state_flag == JY61P_ANGLE_ID) // 角度数据包
        {
            angle_update(); // 更新角度数据
        }
        rt_sem_release(&pid_sem); // 通知PID线程可以运行
        // 这里添加飞控相关的业务逻辑
#if RT_THREAD_SERIAL_DEBUG == 1
        static int count1 = 0;
        if(count1 % 100 == 0)
        {
            rt_kprintf("%d pitch:%.2f roll:%.2f yaw:%.2f\n", count1, pitch, roll, yaw);
            rt_kprintf("%d gyrox:%.2f gyroy:%.2f gyroz:%.2f\n", count1, gyrox, gyroy, gyroz);
            rt_kprintf("%d accx:%.2f accy:%.2f accz:%.2f\n", count1, accx, accy, accz);
            count1++;
        }
#endif
        rt_thread_delay(5);
    }
}

// PID 控制线程入口，负责姿态/速度等闭环控制
extern int fly_unlock_flag; // 解锁标志

void entry2_PID(void *p)
{
    while(1)
    {
        rt_sem_take(&pid_sem, RT_WAITING_FOREVER); // 等待JY901数据更新完成
        if(fly_unlock_flag == 1) // 只有解锁后才允许执行PID控制
        {
#if RT_THREAD_SERIAL_DEBUG == 1
            static int count = 1;
            if(count%6 == 0)  RGB_SET(RGB_COLOR_RED);
            if(count%12 == 1)  RGB_SET(RGB_COLOR_GREEN);
            if(count%18 == 2)  RGB_SET(RGB_COLOR_BLUE);
            count++;
#else
            if(jy901_rx_state_flag == JY61P_GYRO_ID)
            {
                Control_Gyro();
            }
            if(jy901_rx_state_flag == JY61P_ANGLE_ID)
            {
                Control_Angle();
            }
            rt_thread_delay(5);
}

#endif
        }
        // 未解锁时什么都不做，直接等待下一次信号量
  }

// MS53L 和 BMP280 传感器数据采集线程
extern struct rt_semaphore ms53l_sem;

#define MAX_HEIGHT_STEP 200 // 单位mm，每周期最大允许高度变化

float last_height = 0;

void entry3_MS53L_BMP280(void *p)
{
    while(1)
    {
        rt_sem_take(&ms53l_sem, RT_WAITING_FOREVER); // 等待激光测距新数据包
        distance_update(); // 更新ms53l_distance

        // 读取气压计数据
        // Temperature = BMP280_ReadTemperature();
        Pressure = BMP280_ReadPressure();

        // 调用融合接口
        fusion_height = fly_fusion_height(ms53l_distance, Pressure);

#if RT_THREAD_SERIAL_DEBUG == 1
        static int count3 = 0;
        if(count3%50 == 0)
        {
            rt_kprintf("%d fusion_height:%.2f mm  (laser:%.2f mm, pressure:%.2f Pa)\n",
                    count3, fusion_height, ms53l_distance, Pressure);
            count3++;
        }
#endif
    }
}

// 用户/遥控器线程，负责处理用户输入或调试命令
void entry4_USER(void *p)
{
//  int temp=1,flag=1;
//  float wa,wd,sa,sd;
    float speed,target_pitch,target_yaw,target_roll;

    while(1)//解锁部分
    {
        // 处理遥控器指令
        if(at_get_flag() && at_get_state() == AT_STATE_CONTROL)
        {
            at_clear_flag();
			sscanf((char*)rx_4_data,"%f,%f,%f,%f\r\n",&speed,&target_yaw,&target_pitch,&target_roll);
			Motor_Set(speed,speed,speed,speed);

			PID_angle_target_set(target_pitch+2,target_roll-5,target_yaw);
			wa_base = speed;
			wd_base = speed;
			sa_base = speed;
			sd_base = speed;
			if(speed >= 1005 && speed < 1100)
			{
                fly_unlock_flag = 1; // 解锁
				break;
			}
        }
    }
    while(1)
    {
        // 处理遥控器指令
        if(at_get_flag() && at_get_state() == AT_STATE_CONTROL)
        {
            at_clear_flag();
            sscanf((char*)rx_4_data,"%f,%f,%f,%f\r\n",&speed,&target_yaw,&target_roll,&target_pitch);
		    PID_angle_target_set(target_pitch+2,target_roll-5,target_yaw);
		    wa_base = speed;
		    wd_base = speed;
		    sa_base = speed;
		    sd_base = speed;
		    if(speed < 1300)
		    {
			  PID_Param_LowSet();
		    }
		    else if(speed > 1300)
		    {
			  //PID_Clear();
			  PID_Param_HighSet();
		    }
	        if(speed < 1010)
	        {
		        Motor_Set(speed,speed,speed,speed);
	        	continue;
	        }
            else if(fabs(pitch) > 18 || fabs(roll) > 18)
	        {
		        speed = 1001;
	            wa_base = speed;
		        wd_base = speed;
		        sa_base = speed;
		        sd_base = speed;
		        Motor_Set(speed,speed,speed,speed);
	        }
        }

    }

    while(1)        // 处理AT指令（调试）
    {
        if(at_get_flag()&& 0)
        {
            at_state_t state = at_get_state();
            switch(state)
            {
                case AT_STATE_GAS_PEDAL:
                    User_Speed_Base = at_get_user_ad(0); // 获取油门值并写入基速度
                    break;
                case AT_STATE_PWM:
                {
                    int pwm[4];
                    for(int i=0; i<4; i++)
                    {
                        int val = at_get_pwm(i);
                        if(val >= 1000 && val <= 2000)
                            pwm[i] = val;
                        else
                            pwm[i] = 0;
                    }
                    fly_pwm_set(0, pwm[0], pwm[1], pwm[2], pwm[3]);
                    rt_kprintf("Set PWM to %d,%d,%d,%d\n", pwm[0], pwm[1], pwm[2], pwm[3]);
                    break;
                }
                case AT_STATE_TAKEOFF:
                    rt_kprintf("Takeoff!\n");
                    break;
                case AT_STATE_STOP:
                    Fly_Stop(); // 停止飞控
                    rt_kprintf("Stop!\n");
                    break;
                case AT_STATE_UNLOCK:
                    rt_kprintf("Unlock!\n");
                    if(!fly_unlock_flag)
                        Fly_unlock(); // 解锁飞控
                    break;
                default:
                    rt_kprintf("Unknown AT command\n");
                    break;
            }
            at_clear_flag();
        }
    }
}

// MP3 语音模块线程，负责语音播报等功能
void entry5_MP3(void *p)
{
    while(1)
    {
        // TODO: 控制 MP3 播放
        rt_thread_delay(10);
    }
}

// RGB 灯控制线程，负责 RGB 灯的状态更新
void entry6_RGB(void *p)
{
    while(1)
    {
        rt_thread_delay(100); // 延时1秒
#if RT_THREAD_SERIAL_DEBUG == 1
        RGB_SET(RGB_COLOR_RED);
        rt_thread_delay(2000); // 延时1秒
        RGB_SET(RGB_COLOR_GREEN);
        rt_thread_delay(2000); // 延时1秒
        RGB_SET(RGB_COLOR_BLUE);
        rt_thread_delay(2000); // 延时1秒
#endif
    }
}

// 解锁线程，直接控制四路PWM
// 该线程会在解锁后退出，避免占用资源
void entry7_START(void *p)
{
    while(1)
    {
        if(at_get_flag() && at_get_state() == AT_STATE_GAS_PEDAL)
        {
            int throttle = at_get_user_ad(0); // 第一个AD值为油门
            // 只允许油门为2000或1000时写入
            if(throttle == 2000 || throttle == 1000)
            {
                fly_pwm_set(0, throttle, throttle, throttle, throttle);
                // 检测到油门为1000则解锁并退出线程
                if(throttle == 1000)
                {
                    fly_unlock_flag = 1;
                    rt_kprintf("遥控器解锁成功，thread7_START线程已退出\n");
                    at_clear_flag();
                    return;
                }
            }
            at_clear_flag();
        }
        if(fly_unlock_flag == 1)
        {
            return; // 已解锁则直接退出线程
        }
        rt_thread_delay(10);
    }
}
